function [R] = quat2rotmat(q)
mag=0;


	mag=q(1)*q(1) + q(2)*q(2) + q(3)*q(3) + q(4)*q(4);
	if(mag~=1.0)
		mag=1.0/sqrt(mag);
		q(1)=q(1)*mag;
        q(2)=q(2)*mag; 
        q(3)=q(3)*mag; 
        q(4)=q(4)*mag;
    end

  R(1,1)=q(1)*q(1)+q(2)*q(2)-q(3)*q(3)-q(4)*q(4);
	R(1,2)=2*(q(2)*q(3)-q(1)*q(4));
	R(1,3)=2*(q(2)*q(4)+q(1)*q(3));

	R(2,1)=2*(q(2)*q(3)+q(1)*q(4));
	R(2,2)=q(1)*q(1)+q(3)*q(3)-q(2)*q(2)-q(4)*q(4);
	R(2,3)=2*(q(3)*q(4)-q(1)*q(2));

	R(3,1)=2*(q(2)*q(4)-q(1)*q(3));
	R(3,2)=2*(q(3)*q(4)+q(1)*q(2));
	R(3,3)=q(1)*q(1)+q(4)*q(4)-q(2)*q(2)-q(3)*q(3);



end